/*
 * CANopen Service Data Object - client.
 *
 * @file        CO_SDOclient.c
 * @ingroup     CO_SDOclient
 * @author      Janez Paternoster
 * @author      Matej Severkar
 * @copyright   2020 Janez Paternoster
 *
 * This file is part of <https://github.com/CANopenNode/CANopenNode>, a CANopen Stack.
 *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may not use this
 * file except in compliance with the License. You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software distributed under the License is
 * distributed on an "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and limitations under the License.
 */

#include "301/CO_SDOclient.h"

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_ENABLE) != 0

/* verify configuration */
#if CO_CONFIG_SDO_CLI_BUFFER_SIZE < 7U
#error CO_CONFIG_SDO_CLI_BUFFER_SIZE must be set to 7 or more.
#endif
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_ENABLE) == 0
#error CO_CONFIG_FIFO_ENABLE must be enabled.
#endif
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_SEGMENTED) == 0
#error CO_CONFIG_SDO_CLI_SEGMENTED must be enabled.
#endif
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_ALT_READ) == 0
#error CO_CONFIG_FIFO_ALT_READ must be enabled.
#endif
#if ((CO_CONFIG_FIFO)&CO_CONFIG_FIFO_CRC16_CCITT) == 0
#error CO_CONFIG_FIFO_CRC16_CCITT must be enabled.
#endif
#endif

/* default 'protocol switch threshold' size for block transfer */
#ifndef CO_CONFIG_SDO_CLI_PST
#define CO_CONFIG_SDO_CLI_PST 21U
#endif

/*
 * Read received message from CAN module.
 *
 * Function will be called (by CAN receive interrupt) every time, when CAN message with correct identifier
 * will be received. For more information and description of parameters see file CO_driver.h.
 */
static void
CO_SDOclient_receive(void* object, void* msg) {
    CO_SDOclient_t* SDO_C = (CO_SDOclient_t*)object;
    uint8_t DLC = CO_CANrxMsg_readDLC(msg);
    const uint8_t* data = CO_CANrxMsg_readData(msg);

    /* Ignore messages in idle state and messages with wrong length. Ignore
     * message also if previous message was not processed yet and not abort */
    if ((SDO_C->state != CO_SDO_ST_IDLE) && (DLC == 8U) && (!CO_FLAG_READ(SDO_C->CANrxNew) || (data[0] == 0x80U))) {
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
        bool_t state_not_upload_blk_sublock_sreq = (SDO_C->state != CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ);
        bool_t state_not_upload_blk_sublock_crsp = (SDO_C->state != CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP);
        if ((data[0] == 0x80U) /* abort from server */
            || (state_not_upload_blk_sublock_sreq && state_not_upload_blk_sublock_crsp)) {
#endif
            /* copy data and set 'new message' flag */
            (void)memcpy((void*)&SDO_C->CANrxData[0], (const void*)&data[0], 8);
            CO_FLAG_SET(SDO_C->CANrxNew);
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_CALLBACK_PRE) != 0
            /* Optional signal to RTOS, which can resume task, which handles
             * SDO client processing. */
            if (SDO_C->pFunctSignal != NULL) {
                SDO_C->pFunctSignal(SDO_C->functSignalObject);
            }
#endif

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
        } else if (SDO_C->state == CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ) {
            /* block upload, copy data directly */
            CO_SDO_state_t state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ;
            uint8_t seqno = data[0] & 0x7FU;
            SDO_C->timeoutTimer = 0;
            SDO_C->block_timeoutTimer = 0;

            /* verify if sequence number is correct */
            if ((seqno <= SDO_C->block_blksize) && (seqno == (SDO_C->block_seqno + 1U))) {
                SDO_C->block_seqno = seqno;

                /* is this the last segment? */
                if ((data[0] & 0x80U) != 0U) {
                    /* copy data to temporary buffer, because we don't know the number of bytes not containing data */
                    (void)memcpy((void*)&SDO_C->block_dataUploadLast[0], (const void*)&data[1], 7);
                    SDO_C->finished = true;
                    state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP;
                } else {
                    /* Copy data. There is always enough space in fifo buffer,
                     * because block_blksize was calculated before */
                    (void)CO_fifo_write(&SDO_C->bufFifo, &data[1], 7, &SDO_C->block_crc);
                    SDO_C->sizeTran += 7U;
                    /* all segments in sub-block has been transferred */
                    if (seqno == SDO_C->block_blksize) {
                        state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP;
                    }
                }
            }
            /* If message is duplicate or sequence didn't start yet, ignore it. Otherwise seqno is wrong,
             * so break sub-block. Data after last good seqno will be re-transmitted. */
            else if ((seqno != SDO_C->block_seqno) && (SDO_C->block_seqno != 0U)) {
                state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP;
#ifdef CO_DEBUG_SDO_CLIENT
                char msg[80];
                sprintf(msg, "sub-block, rx WRONG: sequno=%02X, previous=%02X", seqno, SDO_C->block_seqno);
                CO_DEBUG_SDO_CLIENT(msg);
#endif
            } else {
                /* MISRA C 2004 14.10 */
#ifdef CO_DEBUG_SDO_CLIENT
                char msg[80];
                sprintf(msg, "sub-block, rx ignored: sequno=%02X, expected=%02X", seqno, SDO_C->block_seqno + 1);
                CO_DEBUG_SDO_CLIENT(msg);
#endif
            }

            /* Is exit from sub-block receive state? */
            if (state != CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ) {
                /* Processing will continue in another thread, so make memory
                 * barrier here with CO_FLAG_CLEAR() call. */
                CO_FLAG_CLEAR(SDO_C->CANrxNew);
                SDO_C->state = state;
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_CALLBACK_PRE) != 0
                /* Optional signal to RTOS, which can resume task, which handles SDO client processing. */
                if (SDO_C->pFunctSignal != NULL) {
                    SDO_C->pFunctSignal(SDO_C->functSignalObject);
                }
#endif
            }
        } else { /* MISRA C 2004 14.10 */
        }

#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */
    }
}

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_OD_DYNAMIC) != 0
/*
 * Custom function for writing OD object _SDO client parameter_
 *
 * For more information see file CO_ODinterface.h, OD_IO_t.
 */
static ODR_t
OD_write_1280(OD_stream_t* stream, const void* buf, OD_size_t count, OD_size_t* countWritten) {
    /* "count" is already verified in *_init() function */
    if ((stream == NULL) || (buf == NULL) || (countWritten == NULL)) {
        return ODR_DEV_INCOMPAT;
    }

    CO_SDOclient_t* SDO_C = (CO_SDOclient_t*)stream->object;

    switch (stream->subIndex) {
        case 0: /* Highest sub-index supported */ return ODR_READONLY; break;

        case 1: { /* COB-ID client -> server */
            uint32_t COB_ID = CO_getUint32(buf);
            uint16_t CAN_ID = (uint16_t)(COB_ID & 0x7FFU);
            uint16_t CAN_ID_cur = (uint16_t)(SDO_C->COB_IDClientToServer & 0x7FFU);
            bool_t valid = (COB_ID & 0x80000000U) == 0U;

            /* SDO client must not be valid when changing COB_ID */
            if (((COB_ID & 0x3FFFF800U) != 0U) || (valid && SDO_C->valid && (CAN_ID != CAN_ID_cur))
                || (valid && CO_IS_RESTRICTED_CAN_ID(CAN_ID))) {
                return ODR_INVALID_VALUE;
            }
            (void)CO_SDOclient_setup(SDO_C, COB_ID, SDO_C->COB_IDServerToClient, SDO_C->nodeIDOfTheSDOServer);
            break;
        }

        case 2: { /* COB-ID server -> client */
            uint32_t COB_ID = CO_getUint32(buf);
            uint16_t CAN_ID = (uint16_t)(COB_ID & 0x7FFU);
            uint16_t CAN_ID_cur = (uint16_t)(SDO_C->COB_IDServerToClient & 0x7FFU);
            bool_t valid = (COB_ID & 0x80000000U) == 0U;

            /* SDO client must not be valid when changing COB_ID */
            if (((COB_ID & 0x3FFFF800U) != 0U) || (valid && SDO_C->valid && (CAN_ID != CAN_ID_cur))
                || (valid && CO_IS_RESTRICTED_CAN_ID(CAN_ID))) {
                return ODR_INVALID_VALUE;
            }
            (void)CO_SDOclient_setup(SDO_C, SDO_C->COB_IDClientToServer, COB_ID, SDO_C->nodeIDOfTheSDOServer);
            break;
        }

        case 3: { /* Node-ID of the SDO server */
            uint8_t nodeId = CO_getUint8(buf);
            if (nodeId > 127U) {
                return ODR_INVALID_VALUE;
            }
            SDO_C->nodeIDOfTheSDOServer = nodeId;
            break;
        }

        default: return ODR_SUB_NOT_EXIST; break;
    }

    /* write value to the original location in the Object Dictionary */
    return OD_writeOriginal(stream, buf, count, countWritten);
}
#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_FLAG_OD_DYNAMIC */

CO_ReturnError_t
CO_SDOclient_init(CO_SDOclient_t* SDO_C, OD_t* OD, OD_entry_t* OD_1280_SDOcliPar, uint8_t nodeId,
                  CO_CANmodule_t* CANdevRx, uint16_t CANdevRxIdx, CO_CANmodule_t* CANdevTx, uint16_t CANdevTxIdx,
                  uint32_t* errInfo) {
    bool_t index_SDOcliPar_min = (OD_getIndex(OD_1280_SDOcliPar) < (uint16_t)(OD_H1280_SDO_CLIENT_1_PARAM));
    bool_t index_SDOcliPar_max = (OD_getIndex(OD_1280_SDOcliPar) > ((uint16_t)(OD_H1280_SDO_CLIENT_1_PARAM) + 0x7FU));

    /* verify arguments */
    if ((SDO_C == NULL) || (OD_1280_SDOcliPar == NULL) || index_SDOcliPar_min || index_SDOcliPar_max
        || (CANdevRx == NULL) || (CANdevTx == NULL)) {
        return CO_ERROR_ILLEGAL_ARGUMENT;
    }

    /* Configure object variables */
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_LOCAL) != 0
    SDO_C->OD = OD;
    SDO_C->nodeId = nodeId;
#endif
    SDO_C->CANdevRx = CANdevRx;
    SDO_C->CANdevRxIdx = CANdevRxIdx;
    SDO_C->CANdevTx = CANdevTx;
    SDO_C->CANdevTxIdx = CANdevTxIdx;
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_CALLBACK_PRE) != 0
    SDO_C->pFunctSignal = NULL;
    SDO_C->functSignalObject = NULL;
#endif

    /* prepare circular fifo buffer */
    CO_fifo_init(&SDO_C->bufFifo, SDO_C->buf, CO_CONFIG_SDO_CLI_BUFFER_SIZE + 1U);

    /* Get parameters from Object Dictionary (initial values) */
    uint8_t maxSubIndex, nodeIDOfTheSDOServer;
    uint32_t COB_IDClientToServer, COB_IDServerToClient;
    ODR_t odRet0 = OD_get_u8(OD_1280_SDOcliPar, 0, &maxSubIndex, true);
    ODR_t odRet1 = OD_get_u32(OD_1280_SDOcliPar, 1, &COB_IDClientToServer, true);
    ODR_t odRet2 = OD_get_u32(OD_1280_SDOcliPar, 2, &COB_IDServerToClient, true);
    ODR_t odRet3 = OD_get_u8(OD_1280_SDOcliPar, 3, &nodeIDOfTheSDOServer, true);

    if ((odRet0 != ODR_OK) || (maxSubIndex != 3U) || (odRet1 != ODR_OK) || (odRet2 != ODR_OK) || (odRet3 != ODR_OK)) {
        if (errInfo != NULL) {
            *errInfo = OD_getIndex(OD_1280_SDOcliPar);
        }
        return CO_ERROR_OD_PARAMETERS;
    }

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_OD_DYNAMIC) != 0
    SDO_C->OD_1280_extension.object = SDO_C;
    SDO_C->OD_1280_extension.read = OD_readOriginal;
    SDO_C->OD_1280_extension.write = OD_write_1280;
    ODR_t odRetE = OD_extension_init(OD_1280_SDOcliPar, &SDO_C->OD_1280_extension);
    if (odRetE != ODR_OK) {
        if (errInfo != NULL) {
            *errInfo = OD_getIndex(OD_1280_SDOcliPar);
        }
        return CO_ERROR_OD_PARAMETERS;
    }

    /* set to zero to make sure CO_SDOclient_setup() will reconfigure CAN */
    SDO_C->COB_IDClientToServer = 0;
    SDO_C->COB_IDServerToClient = 0;
#endif

    CO_SDO_return_t cliSetupRet = CO_SDOclient_setup(SDO_C, COB_IDClientToServer, COB_IDServerToClient,
                                                     nodeIDOfTheSDOServer);

    if (cliSetupRet != CO_SDO_RT_ok_communicationEnd) {
        return CO_ERROR_ILLEGAL_ARGUMENT;
    }

    return CO_ERROR_NO;
}

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_CALLBACK_PRE) != 0
void
CO_SDOclient_initCallbackPre(CO_SDOclient_t* SDOclient, void* object, void (*pFunctSignal)(void* object)) {
    if (SDOclient != NULL) {
        SDOclient->functSignalObject = object;
        SDOclient->pFunctSignal = pFunctSignal;
    }
}
#endif

#if (((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_LOCAL) != 0) && defined CO_BIG_ENDIAN
static inline void
reverseBytes(void* start, OD_size_t size) {
    uint8_t* lo = (uint8_t*)start;
    uint8_t* hi = (uint8_t*)start + size - 1;
    uint8_t swap;
    while (lo < hi) {
        swap = *lo;
        *lo++ = *hi;
        *hi-- = swap;
    }
}
#endif

CO_SDO_return_t
CO_SDOclient_setup(CO_SDOclient_t* SDO_C, uint32_t COB_IDClientToServer, uint32_t COB_IDServerToClient,
                   uint8_t nodeIDOfTheSDOServer) {
    /* verify parameters */
    if (SDO_C == NULL) {
        return CO_SDO_RT_wrongArguments;
    }

    /* Configure object variables */
    SDO_C->state = CO_SDO_ST_IDLE;
    CO_FLAG_CLEAR(SDO_C->CANrxNew);
    SDO_C->nodeIDOfTheSDOServer = nodeIDOfTheSDOServer;

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_OD_DYNAMIC) != 0
    /* proceed only, if parameters change */
    if ((COB_IDClientToServer == SDO_C->COB_IDClientToServer)
        && (COB_IDServerToClient == SDO_C->COB_IDServerToClient)) {
        return CO_SDO_RT_ok_communicationEnd;
    }
    /* store variables */
    SDO_C->COB_IDClientToServer = COB_IDClientToServer;
    SDO_C->COB_IDServerToClient = COB_IDServerToClient;
#endif

    /* verify valid bit */
    uint16_t CanIdC2S = ((COB_IDClientToServer & 0x80000000UL) == 0U) ? (uint16_t)(COB_IDClientToServer & 0x7FFU) : 0U;
    uint16_t CanIdS2C = ((COB_IDServerToClient & 0x80000000UL) == 0U) ? (uint16_t)(COB_IDServerToClient & 0x7FFU) : 0U;
    if ((CanIdC2S != 0U) && (CanIdS2C != 0U)) {
        SDO_C->valid = true;
    } else {
        CanIdC2S = 0;
        CanIdS2C = 0;
        SDO_C->valid = false;
    }

    /* configure SDO client CAN reception */
    CO_ReturnError_t ret = CO_CANrxBufferInit(SDO_C->CANdevRx, SDO_C->CANdevRxIdx, CanIdS2C, 0x7FF, false, (void*)SDO_C,
                                              CO_SDOclient_receive);

    /* configure SDO client CAN transmission */
    SDO_C->CANtxBuff = CO_CANtxBufferInit(SDO_C->CANdevTx, SDO_C->CANdevTxIdx, CanIdC2S, false, 8, false);

    if ((ret != CO_ERROR_NO) || (SDO_C->CANtxBuff == NULL)) {
        SDO_C->valid = false;
        return CO_SDO_RT_wrongArguments;
    }

    return CO_SDO_RT_ok_communicationEnd;
}

/******************************************************************************
 * DOWNLOAD                                                                   *
 ******************************************************************************/
CO_SDO_return_t
CO_SDOclientDownloadInitiate(CO_SDOclient_t* SDO_C, uint16_t index, uint8_t subIndex, size_t sizeIndicated,
                             uint16_t SDOtimeoutTime_ms, bool_t blockEnable) {
    /* verify parameters */
    if ((SDO_C == NULL) || !SDO_C->valid) {
        return CO_SDO_RT_wrongArguments;
    }

    /* save parameters */
    SDO_C->index = index;
    SDO_C->subIndex = subIndex;
    SDO_C->sizeInd = sizeIndicated;
    SDO_C->sizeTran = 0;
    SDO_C->finished = false;
    SDO_C->SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 1000U;
    SDO_C->timeoutTimer = 0;
    CO_fifo_reset(&SDO_C->bufFifo);

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_LOCAL) != 0
    /* if node-ID of the SDO server is the same as node-ID of this node, then transfer data within this node */
    if ((SDO_C->OD != NULL) && (SDO_C->nodeId != 0U) && (SDO_C->nodeIDOfTheSDOServer == SDO_C->nodeId)) {
        SDO_C->OD_IO.write = NULL;
        SDO_C->state = CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER;
    } else
#endif
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
        if (blockEnable && ((sizeIndicated == 0U) || (sizeIndicated > (size_t)(CO_CONFIG_SDO_CLI_PST)))) {
        SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ;
    } else
#endif
    {
        SDO_C->state = CO_SDO_ST_DOWNLOAD_INITIATE_REQ;
    }

    CO_FLAG_CLEAR(SDO_C->CANrxNew);

    return CO_SDO_RT_ok_communicationEnd;
}

void
CO_SDOclientDownloadInitSize(CO_SDOclient_t* SDO_C, size_t sizeIndicated) {
    if (SDO_C != NULL) {
        SDO_C->sizeInd = sizeIndicated;
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
        if ((SDO_C->state == CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ) && (sizeIndicated > 0U)
            && (sizeIndicated <= (size_t)(CO_CONFIG_SDO_CLI_PST))) {
            SDO_C->state = CO_SDO_ST_DOWNLOAD_INITIATE_REQ;
        }
#endif
    }
}

size_t
CO_SDOclientDownloadBufWrite(CO_SDOclient_t* SDO_C, const uint8_t* buf, size_t count) {
    size_t ret = 0;
    if ((SDO_C != NULL) && (buf != NULL)) {
        ret = CO_fifo_write(&SDO_C->bufFifo, buf, count, NULL);
    }
    return ret;
}

CO_SDO_return_t
CO_SDOclientDownload(CO_SDOclient_t* SDO_C, uint32_t timeDifference_us, bool_t send_abort, bool_t bufferPartial,
                     CO_SDO_abortCode_t* SDOabortCode, size_t* sizeTransferred, uint32_t* timerNext_us) {
    (void)timerNext_us;
    (void)bufferPartial; /* may be unused */

    CO_SDO_return_t ret = CO_SDO_RT_waitingResponse;
    CO_SDO_abortCode_t abortCode = CO_SDO_AB_NONE;

    if ((SDO_C == NULL) || !SDO_C->valid) {
        abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
        ret = CO_SDO_RT_wrongArguments;
    } else if (SDO_C->state == CO_SDO_ST_IDLE) {
        ret = CO_SDO_RT_ok_communicationEnd;
    }
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_LOCAL) != 0
    /* Transfer data locally */
    else if ((SDO_C->state == CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER) && !send_abort) {
        /* search object dictionary in first pass */
        if (SDO_C->OD_IO.write == NULL) {
            ODR_t odRet;

            odRet = OD_getSub(OD_find(SDO_C->OD, SDO_C->index), SDO_C->subIndex, &SDO_C->OD_IO, false);

            if (odRet != ODR_OK) {
                abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
                ret = CO_SDO_RT_endedWithClientAbort;
            } else if ((SDO_C->OD_IO.stream.attribute & (OD_attr_t)ODA_SDO_RW) == 0U) {
                abortCode = CO_SDO_AB_UNSUPPORTED_ACCESS;
                ret = CO_SDO_RT_endedWithClientAbort;
            } else if ((SDO_C->OD_IO.stream.attribute & (OD_attr_t)ODA_SDO_W) == 0U) {
                abortCode = CO_SDO_AB_READONLY;
                ret = CO_SDO_RT_endedWithClientAbort;
            } else if (SDO_C->OD_IO.write == NULL) {
                abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
                ret = CO_SDO_RT_endedWithClientAbort;
            } else { /* MISRA C 2004 14.10 */
            }
        }
        /* write data, in several passes if necessary */
        if (SDO_C->OD_IO.write != NULL) {
            size_t count = CO_fifo_getOccupied(&SDO_C->bufFifo);
            uint8_t buf[CO_CONFIG_SDO_CLI_BUFFER_SIZE + 2U];

            (void)CO_fifo_read(&SDO_C->bufFifo, buf, count, NULL);
            SDO_C->sizeTran += count;

            /* error: no data */
            if ((count == 0U) || (count > CO_CONFIG_SDO_CLI_BUFFER_SIZE)) {
                abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
                ret = CO_SDO_RT_endedWithClientAbort;
            }
            /* verify if sizeTran is too large */
            else if ((SDO_C->sizeInd > 0U) && (SDO_C->sizeTran > SDO_C->sizeInd)) {
                SDO_C->sizeTran -= count;
                abortCode = CO_SDO_AB_DATA_LONG;
                ret = CO_SDO_RT_endedWithClientAbort;
            }
            /* Verify sizeTran is too small in last segment of data */
            else if (!bufferPartial && (SDO_C->sizeInd > 0U) && (SDO_C->sizeTran < SDO_C->sizeInd)) {
                abortCode = CO_SDO_AB_DATA_SHORT;
                ret = CO_SDO_RT_endedWithClientAbort;
            }
            /* is the last segment of data? */
            else if (!bufferPartial) {
#ifdef CO_BIG_ENDIAN
                /* swap int16_t .. uint64_t data if necessary */
                if ((SDO_C->OD_IO.stream.attribute & ODA_MB) != 0) {
                    reverseBytes(buf, count);
                }
#endif
                OD_size_t sizeInOd = SDO_C->OD_IO.stream.dataLength;

                /* If dataType is string, then size of data downloaded may be shorter than size of
                 * OD data buffer. If so, add two zero bytes to terminate (unicode) string. Shorten
                 * also OD data size, (temporary, send info about EOF into OD_IO.write) */
                if (((SDO_C->OD_IO.stream.attribute & (OD_attr_t)ODA_STR) != 0U)
                    && ((sizeInOd == 0U) || (SDO_C->sizeTran < sizeInOd))) {
                    buf[count] = 0;
                    count++;
                    SDO_C->sizeTran++;
                    if ((sizeInOd == 0U) || (sizeInOd > SDO_C->sizeTran)) {
                        buf[count] = 0;
                        count++;
                        SDO_C->sizeTran++;
                    }
                    SDO_C->OD_IO.stream.dataLength = (OD_size_t)SDO_C->sizeTran;
                }
                /* Indicate OD data size, if necessary. Used for EOF check. */
                else if (sizeInOd == 0U) {
                    SDO_C->OD_IO.stream.dataLength = (OD_size_t)SDO_C->sizeTran;
                }
                /* Verify if size of data downloaded matches data size in OD. */
                else if (SDO_C->sizeTran != sizeInOd) {
                    abortCode = (SDO_C->sizeTran > sizeInOd) ? CO_SDO_AB_DATA_LONG : CO_SDO_AB_DATA_SHORT;
                    ret = CO_SDO_RT_endedWithClientAbort;
                } else { /* MISRA C 2004 14.10 */
                }
            } else { /* MISRA C 2004 14.10 */
            }

            if (abortCode == CO_SDO_AB_NONE) {
                OD_size_t countWritten = 0;

                /* write data to Object Dictionary */
                CO_LOCK_OD(SDO_C->CANdevTx);
                ODR_t odRet = SDO_C->OD_IO.write(&SDO_C->OD_IO.stream, buf, (OD_size_t)count, &countWritten);
                CO_UNLOCK_OD(SDO_C->CANdevTx);

                /* verify for errors in write */
                if ((odRet != ODR_OK) && (odRet != ODR_PARTIAL)) {
                    abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
                    ret = CO_SDO_RT_endedWithServerAbort;
                }
                /* error if OD variable was written completely, but SDO download still has data */
                else if (bufferPartial && (odRet == ODR_OK)) {
                    abortCode = CO_SDO_AB_DATA_LONG;
                    ret = CO_SDO_RT_endedWithClientAbort;
                }
                /* is end of transfer? */
                else if (!bufferPartial) {
                    /* error if OD variable was not written completely, but SDO download finished */
                    if (odRet == ODR_PARTIAL) {
                        abortCode = CO_SDO_AB_DATA_SHORT;
                        ret = CO_SDO_RT_endedWithClientAbort;
                    }
                    /* data transfer finished successfully */
                    else {
                        ret = CO_SDO_RT_ok_communicationEnd;
                    }
                } else {
                    ret = CO_SDO_RT_waitingLocalTransfer;
                }
            }
        }

        if (ret != CO_SDO_RT_waitingLocalTransfer) {
            SDO_C->state = CO_SDO_ST_IDLE;
        }
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_TIMERNEXT) != 0
        /* Inform OS to call this function again without delay. */
        else if (timerNext_us != NULL) {
            *timerNext_us = 0;
        } else { /* MISRA C 2004 14.10 */
        }
#endif
    }
#endif /* CO_CONFIG_SDO_CLI_LOCAL */
    /* CAN data received */
    else if (CO_FLAG_READ(SDO_C->CANrxNew)) {
        /* is SDO abort */
        if (SDO_C->CANrxData[0] == 0x80U) {
            uint32_t code;
            (void)memcpy((void*)(&code), (const void*)(&SDO_C->CANrxData[4]), sizeof(code));
            abortCode = (CO_SDO_abortCode_t)CO_SWAP_32(code);
            SDO_C->state = CO_SDO_ST_IDLE;
            ret = CO_SDO_RT_endedWithServerAbort;
        } else if (send_abort) {
            abortCode = (SDOabortCode != NULL) ? *SDOabortCode : CO_SDO_AB_DEVICE_INCOMPAT;
            SDO_C->state = CO_SDO_ST_ABORT;
        } else {
            switch (SDO_C->state) {
                case CO_SDO_ST_DOWNLOAD_INITIATE_RSP: {
                    if (SDO_C->CANrxData[0] == 0x60U) {
                        /* verify index and subindex */
                        uint16_t index;
                        uint8_t subindex;
                        index = ((uint16_t)SDO_C->CANrxData[2]) << 8;
                        index |= SDO_C->CANrxData[1];
                        subindex = SDO_C->CANrxData[3];
                        if ((index != SDO_C->index) || (subindex != SDO_C->subIndex)) {
                            abortCode = CO_SDO_AB_PRAM_INCOMPAT;
                            SDO_C->state = CO_SDO_ST_ABORT;
                            break;
                        }

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_SEGMENTED) != 0
                        if (SDO_C->finished) {
                            /* expedited transfer */
                            SDO_C->state = CO_SDO_ST_IDLE;
                            ret = CO_SDO_RT_ok_communicationEnd;
                        } else {
                            /* segmented transfer - prepare the first segment */
                            SDO_C->toggle = 0x00;
                            SDO_C->state = CO_SDO_ST_DOWNLOAD_SEGMENT_REQ;
                        }
#else
                        /* expedited transfer */
                        SDO_C->state = CO_SDO_ST_IDLE;
                        ret = CO_SDO_RT_ok_communicationEnd;
#endif
                    } else {
                        abortCode = CO_SDO_AB_CMD;
                        SDO_C->state = CO_SDO_ST_ABORT;
                    }
                    break;
                }

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_SEGMENTED) != 0
                case CO_SDO_ST_DOWNLOAD_SEGMENT_RSP: {
                    if ((SDO_C->CANrxData[0] & 0xEFU) == 0x20U) {
                        /* verify and alternate toggle bit */
                        uint8_t toggle = SDO_C->CANrxData[0] & 0x10U;
                        if (toggle != SDO_C->toggle) {
                            abortCode = CO_SDO_AB_TOGGLE_BIT;
                            SDO_C->state = CO_SDO_ST_ABORT;
                            break;
                        }
                        SDO_C->toggle = (toggle == 0x00U) ? 0x10U : 0x00U;

                        /* is end of transfer? */
                        if (SDO_C->finished) {
                            SDO_C->state = CO_SDO_ST_IDLE;
                            ret = CO_SDO_RT_ok_communicationEnd;
                        } else {
                            SDO_C->state = CO_SDO_ST_DOWNLOAD_SEGMENT_REQ;
                        }
                    } else {
                        abortCode = CO_SDO_AB_CMD;
                        SDO_C->state = CO_SDO_ST_ABORT;
                    }
                    break;
                }
#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED */

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
                case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP: {
                    if ((SDO_C->CANrxData[0] & 0xFBU) == 0xA0U) {
                        /* verify index and subindex */
                        uint16_t index;
                        uint8_t subindex;
                        index = ((uint16_t)SDO_C->CANrxData[2]) << 8;
                        index |= SDO_C->CANrxData[1];
                        subindex = SDO_C->CANrxData[3];
                        if ((index != SDO_C->index) || (subindex != SDO_C->subIndex)) {
                            abortCode = CO_SDO_AB_PRAM_INCOMPAT;
                            SDO_C->state = CO_SDO_ST_ABORT;
                            break;
                        }

                        SDO_C->block_crc = 0;
                        SDO_C->block_blksize = SDO_C->CANrxData[4];
                        if ((SDO_C->block_blksize < 1U) || (SDO_C->block_blksize > 127U)) {
                            SDO_C->block_blksize = 127;
                        }
                        SDO_C->block_seqno = 0;
                        (void)CO_fifo_altBegin(&SDO_C->bufFifo, 0);
                        SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ;
                    } else {
                        abortCode = CO_SDO_AB_CMD;
                        SDO_C->state = CO_SDO_ST_ABORT;
                    }
                    break;
                }

                case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ:
                case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP: {
                    if (SDO_C->CANrxData[0] == 0xA2U) {
                        /* check number of segments */
                        if (SDO_C->CANrxData[1] < SDO_C->block_seqno) {
                            /* NOT all segments transferred successfully. Re-transmit data after erroneous segment. */
                            size_t cntFailed = (size_t)(SDO_C->block_seqno) - (size_t)(SDO_C->CANrxData[1]);
                            cntFailed = (cntFailed * 7U) - SDO_C->block_noData;
                            SDO_C->sizeTran -= cntFailed;
                            (void)CO_fifo_altBegin(&SDO_C->bufFifo, (size_t)SDO_C->CANrxData[1] * 7U);
                            SDO_C->finished = false;
                        } else if (SDO_C->CANrxData[1] > SDO_C->block_seqno) {
                            /* something strange from server, break transmission */
                            abortCode = CO_SDO_AB_CMD;
                            SDO_C->state = CO_SDO_ST_ABORT;
                            break;
                        } else { /* MISRA C 2004 14.10 */
                        }

                        /* confirm successfully transmitted data */
                        CO_fifo_altFinish(&SDO_C->bufFifo, &SDO_C->block_crc);

                        if (SDO_C->finished) {
                            SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_END_REQ;
                        } else {
                            SDO_C->block_blksize = SDO_C->CANrxData[2];
                            SDO_C->block_seqno = 0;
                            (void)CO_fifo_altBegin(&SDO_C->bufFifo, 0);
                            SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ;
                        }
                    } else {
                        abortCode = CO_SDO_AB_CMD;
                        SDO_C->state = CO_SDO_ST_ABORT;
                    }
                    break;
                }

                case CO_SDO_ST_DOWNLOAD_BLK_END_RSP: {
                    if (SDO_C->CANrxData[0] == 0xA1U) {
                        /*  SDO block download successfully transferred */
                        SDO_C->state = CO_SDO_ST_IDLE;
                        ret = CO_SDO_RT_ok_communicationEnd;
                    } else {
                        abortCode = CO_SDO_AB_CMD;
                        SDO_C->state = CO_SDO_ST_ABORT;
                    }
                    break;
                }
#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_SEGMENTED) == 0
                case CO_SDO_ST_DOWNLOAD_SEGMENT_RSP:
#endif
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) == 0
                case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP:
                case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ:
                case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP:
                case CO_SDO_ST_DOWNLOAD_BLK_END_RSP:
#endif
                case CO_SDO_ST_IDLE:
                case CO_SDO_ST_ABORT:
                case CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER:
                case CO_SDO_ST_DOWNLOAD_INITIATE_REQ:
                case CO_SDO_ST_DOWNLOAD_SEGMENT_REQ:
                case CO_SDO_ST_UPLOAD_LOCAL_TRANSFER:
                case CO_SDO_ST_UPLOAD_INITIATE_REQ:
                case CO_SDO_ST_UPLOAD_INITIATE_RSP:
                case CO_SDO_ST_UPLOAD_SEGMENT_REQ:
                case CO_SDO_ST_UPLOAD_SEGMENT_RSP:
                case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ:
                case CO_SDO_ST_DOWNLOAD_BLK_END_REQ:
                case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ:
                case CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP:
                case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2:
                case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ:
                case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP:
                case CO_SDO_ST_UPLOAD_BLK_END_SREQ:
                case CO_SDO_ST_UPLOAD_BLK_END_CRSP:
                default: {
                    abortCode = CO_SDO_AB_CMD;
                    SDO_C->state = CO_SDO_ST_ABORT;
                    break;
                }
            }
        }
        SDO_C->timeoutTimer = 0;
        timeDifference_us = 0;
        CO_FLAG_CLEAR(SDO_C->CANrxNew);
    } else if (send_abort) {
        abortCode = (SDOabortCode != NULL) ? *SDOabortCode : CO_SDO_AB_DEVICE_INCOMPAT;
        SDO_C->state = CO_SDO_ST_ABORT;
    } else { /* MISRA C 2004 14.10 */
    }

    /* Timeout timers and transmit bufferFull flag */
    if (ret == CO_SDO_RT_waitingResponse) {
        if (SDO_C->timeoutTimer < SDO_C->SDOtimeoutTime_us) {
            SDO_C->timeoutTimer += timeDifference_us;
        }
        if (SDO_C->timeoutTimer >= SDO_C->SDOtimeoutTime_us) {
            abortCode = CO_SDO_AB_TIMEOUT;
            SDO_C->state = CO_SDO_ST_ABORT;
        }
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_TIMERNEXT) != 0
        else if (timerNext_us != NULL) {
            /* check again after timeout time elapsed */
            uint32_t diff = SDO_C->SDOtimeoutTime_us - SDO_C->timeoutTimer;
            if (*timerNext_us > diff) {
                *timerNext_us = diff;
            }
        } else { /* MISRA C 2004 14.10 */
        }
#endif
        if (SDO_C->CANtxBuff->bufferFull) {
            ret = CO_SDO_RT_transmittBufferFull;
        }
    }

    /* Transmit CAN data */
    if (ret == CO_SDO_RT_waitingResponse) {
        size_t count;
        (void)memset((void*)&SDO_C->CANtxBuff->data[0], 0, 8);

        switch (SDO_C->state) {
            case CO_SDO_ST_DOWNLOAD_INITIATE_REQ: {
                SDO_C->CANtxBuff->data[0] = 0x20;
                SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index;
                SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8);
                SDO_C->CANtxBuff->data[3] = SDO_C->subIndex;

                /* get count of data bytes to transfer */
                count = CO_fifo_getOccupied(&SDO_C->bufFifo);

                /* is expedited transfer, <= 4bytes of data */
                if (((SDO_C->sizeInd == 0U) && (count <= 4U)) || ((SDO_C->sizeInd > 0U) && (SDO_C->sizeInd <= 4U))) {
                    SDO_C->CANtxBuff->data[0] |= 0x02U;

                    /* verify length, indicate data size */
                    if ((count == 0U) || ((SDO_C->sizeInd > 0U) && (SDO_C->sizeInd != count))) {
                        SDO_C->state = CO_SDO_ST_IDLE;
                        abortCode = CO_SDO_AB_TYPE_MISMATCH;
                        ret = CO_SDO_RT_endedWithClientAbort;
                        break;
                    }
                    if (SDO_C->sizeInd > 0U) {
                        SDO_C->CANtxBuff->data[0] |= (uint8_t)(0x01U | ((4U - count) << 2));
                    }

                    /* copy data */
                    (void)CO_fifo_read(&SDO_C->bufFifo, &SDO_C->CANtxBuff->data[4], count, NULL);
                    SDO_C->sizeTran = count;
                    SDO_C->finished = true;
                } else {
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_SEGMENTED) != 0
                    /* segmented transfer, indicate data size */
                    if (SDO_C->sizeInd > 0U) {
                        uint32_t size = CO_SWAP_32((uint32_t)SDO_C->sizeInd);
                        SDO_C->CANtxBuff->data[0] |= 0x01U;
                        (void)memcpy((void*)(&SDO_C->CANtxBuff->data[4]), (const void*)(&size), sizeof(size));
                    }
#else
                    SDO_C->state = CO_SDO_ST_IDLE;
                    abortCode = CO_SDO_AB_UNSUPPORTED_ACCESS;
                    ret = CO_SDO_RT_endedWithClientAbort;
                    break;
#endif
                }

                /* reset timeout timer and send message */
                SDO_C->timeoutTimer = 0;
                (void)CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
                SDO_C->state = CO_SDO_ST_DOWNLOAD_INITIATE_RSP;
                break;
            }

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_SEGMENTED) != 0
            case CO_SDO_ST_DOWNLOAD_SEGMENT_REQ: {
                /* fill data bytes */
                count = CO_fifo_read(&SDO_C->bufFifo, &SDO_C->CANtxBuff->data[1], 7, NULL);

                /* verify if sizeTran is too large */
                SDO_C->sizeTran += count;
                if ((SDO_C->sizeInd > 0U) && (SDO_C->sizeTran > SDO_C->sizeInd)) {
                    SDO_C->sizeTran -= count;
                    abortCode = CO_SDO_AB_DATA_LONG;
                    SDO_C->state = CO_SDO_ST_ABORT;
                    break;
                }

                /* SDO command specifier */
                SDO_C->CANtxBuff->data[0] = (uint8_t)(SDO_C->toggle | ((7U - count) << 1));

                /* is end of transfer? Verify also sizeTran */
                if ((CO_fifo_getOccupied(&SDO_C->bufFifo) == 0U) && !bufferPartial) {
                    if ((SDO_C->sizeInd > 0U) && (SDO_C->sizeTran < SDO_C->sizeInd)) {
                        abortCode = CO_SDO_AB_DATA_SHORT;
                        SDO_C->state = CO_SDO_ST_ABORT;
                        break;
                    }
                    SDO_C->CANtxBuff->data[0] |= 0x01U;
                    SDO_C->finished = true;
                }

                /* reset timeout timer and send message */
                SDO_C->timeoutTimer = 0;
                (void)CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
                SDO_C->state = CO_SDO_ST_DOWNLOAD_SEGMENT_RSP;
                break;
            }
#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED */

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
            case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ: {
                SDO_C->CANtxBuff->data[0] = 0xC4;
                SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index;
                SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8);
                SDO_C->CANtxBuff->data[3] = SDO_C->subIndex;

                /* indicate data size */
                if (SDO_C->sizeInd > 0U) {
                    uint32_t size = CO_SWAP_32((uint32_t)SDO_C->sizeInd);
                    SDO_C->CANtxBuff->data[0] |= 0x02U;
                    (void)memcpy((void*)(&SDO_C->CANtxBuff->data[4]), (const void*)(&size), sizeof(size));
                }

                /* reset timeout timer and send message */
                SDO_C->timeoutTimer = 0;
                (void)CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
                SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP;
                break;
            }

            case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ: {
                if ((CO_fifo_altGetOccupied(&SDO_C->bufFifo) < 7U) && bufferPartial) {
                    /* wait until data are refilled */
                    break;
                }
                SDO_C->block_seqno++;
                SDO_C->CANtxBuff->data[0] = SDO_C->block_seqno;

                /* get up to 7 data bytes */
                count = CO_fifo_altRead(&SDO_C->bufFifo, &SDO_C->CANtxBuff->data[1], 7);
                SDO_C->block_noData = (uint8_t)(7U - count);

                /* verify if sizeTran is too large */
                SDO_C->sizeTran += count;
                if ((SDO_C->sizeInd > 0U) && (SDO_C->sizeTran > SDO_C->sizeInd)) {
                    SDO_C->sizeTran -= count;
                    abortCode = CO_SDO_AB_DATA_LONG;
                    SDO_C->state = CO_SDO_ST_ABORT;
                    break;
                }

                /* is end of transfer? Verify also sizeTran */
                if ((CO_fifo_altGetOccupied(&SDO_C->bufFifo) == 0U) && !bufferPartial) {
                    if ((SDO_C->sizeInd > 0U) && (SDO_C->sizeTran < SDO_C->sizeInd)) {
                        abortCode = CO_SDO_AB_DATA_SHORT;
                        SDO_C->state = CO_SDO_ST_ABORT;
                        break;
                    }
                    SDO_C->CANtxBuff->data[0] |= 0x80U;
                    SDO_C->finished = true;
                    SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP;
                }
                /* are all segments in current block transferred? */
                else if (SDO_C->block_seqno >= SDO_C->block_blksize) {
                    SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP;
                }
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_TIMERNEXT) != 0
                else {
                    /* Inform OS to call this function again without delay. */
                    if (timerNext_us != NULL) {
                        *timerNext_us = 0;
                    }
                }
#endif
                /* reset timeout timer and send message */
                SDO_C->timeoutTimer = 0;
                (void)CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
                break;
            }

            case CO_SDO_ST_DOWNLOAD_BLK_END_REQ: {
                SDO_C->CANtxBuff->data[0] = (uint8_t)(0xC1U | (SDO_C->block_noData << 2));
                SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->block_crc;
                SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->block_crc >> 8);

                /* reset timeout timer and send message */
                SDO_C->timeoutTimer = 0;
                (void)CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
                SDO_C->state = CO_SDO_ST_DOWNLOAD_BLK_END_RSP;
                break;
            }
#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_SEGMENTED) == 0
            case CO_SDO_ST_DOWNLOAD_SEGMENT_REQ:
#endif
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) == 0
            case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ:
            case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ:
            case CO_SDO_ST_DOWNLOAD_BLK_END_REQ:
#endif
            case CO_SDO_ST_IDLE:
            case CO_SDO_ST_ABORT:
            case CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER:
            case CO_SDO_ST_DOWNLOAD_INITIATE_RSP:
            case CO_SDO_ST_DOWNLOAD_SEGMENT_RSP:
            case CO_SDO_ST_UPLOAD_LOCAL_TRANSFER:
            case CO_SDO_ST_UPLOAD_INITIATE_REQ:
            case CO_SDO_ST_UPLOAD_INITIATE_RSP:
            case CO_SDO_ST_UPLOAD_SEGMENT_REQ:
            case CO_SDO_ST_UPLOAD_SEGMENT_RSP:
            case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP:
            case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP:
            case CO_SDO_ST_DOWNLOAD_BLK_END_RSP:
            case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ:
            case CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP:
            case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2:
            case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ:
            case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP:
            case CO_SDO_ST_UPLOAD_BLK_END_SREQ:
            case CO_SDO_ST_UPLOAD_BLK_END_CRSP:
            default: {
                /* none */
                break;
            }
        }
    }

    if (ret == CO_SDO_RT_waitingResponse) {
        if (SDO_C->state == CO_SDO_ST_ABORT) {
            uint32_t code = CO_SWAP_32((uint32_t)abortCode);
            /* Send SDO abort message */
            SDO_C->CANtxBuff->data[0] = 0x80;
            SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index;
            SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8);
            SDO_C->CANtxBuff->data[3] = SDO_C->subIndex;

            (void)memcpy((void*)(&SDO_C->CANtxBuff->data[4]), (const void*)(&code), sizeof(code));
            (void)CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
            SDO_C->state = CO_SDO_ST_IDLE;
            ret = CO_SDO_RT_endedWithClientAbort;
        }
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
        else if (SDO_C->state == CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ) {
            ret = CO_SDO_RT_blockDownldInProgress;
        } else { /* MISRA C 2004 14.10 */
        }
#endif
    }

    if (sizeTransferred != NULL) {
        *sizeTransferred = SDO_C->sizeTran;
    }
    if (SDOabortCode != NULL) {
        *SDOabortCode = abortCode;
    }

    return ret;
}

/******************************************************************************
 * UPLOAD                                                                     *
 ******************************************************************************/
CO_SDO_return_t
CO_SDOclientUploadInitiate(CO_SDOclient_t* SDO_C, uint16_t index, uint8_t subIndex, uint16_t SDOtimeoutTime_ms,
                           bool_t blockEnable) {
    /* verify parameters */
    if ((SDO_C == NULL) || !SDO_C->valid) {
        return CO_SDO_RT_wrongArguments;
    }

    /* save parameters */
    SDO_C->index = index;
    SDO_C->subIndex = subIndex;
    SDO_C->sizeInd = 0;
    SDO_C->sizeTran = 0;
    SDO_C->finished = false;
    CO_fifo_reset(&SDO_C->bufFifo);
    SDO_C->SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 1000U;
    SDO_C->timeoutTimer = 0;
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
    SDO_C->block_SDOtimeoutTime_us = (uint32_t)SDOtimeoutTime_ms * 700U;
#endif

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_LOCAL) != 0
    /* if node-ID of the SDO server is the same as node-ID of this node, then transfer data within this node */
    if (((SDO_C->OD != NULL) && (SDO_C->nodeId != 0U)) && (SDO_C->nodeIDOfTheSDOServer == SDO_C->nodeId)) {
        SDO_C->OD_IO.read = NULL;
        SDO_C->state = CO_SDO_ST_UPLOAD_LOCAL_TRANSFER;
    } else
#endif
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
        if (blockEnable) {
        SDO_C->state = CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ;
    } else
#endif
    {
        SDO_C->state = CO_SDO_ST_UPLOAD_INITIATE_REQ;
    }

    CO_FLAG_CLEAR(SDO_C->CANrxNew);

    return CO_SDO_RT_ok_communicationEnd;
}

CO_SDO_return_t
CO_SDOclientUpload(CO_SDOclient_t* SDO_C, uint32_t timeDifference_us, bool_t send_abort,
                   CO_SDO_abortCode_t* SDOabortCode, size_t* sizeIndicated, size_t* sizeTransferred,
                   uint32_t* timerNext_us) {
    (void)timerNext_us; /* may be unused */

    CO_SDO_return_t ret = CO_SDO_RT_waitingResponse;
    CO_SDO_abortCode_t abortCode = CO_SDO_AB_NONE;

    if ((SDO_C == NULL) || !SDO_C->valid) {
        abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
        ret = CO_SDO_RT_wrongArguments;
    } else if (SDO_C->state == CO_SDO_ST_IDLE) {
        ret = CO_SDO_RT_ok_communicationEnd;
    }
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_LOCAL) != 0
    /* Transfer data locally */
    else if ((SDO_C->state == CO_SDO_ST_UPLOAD_LOCAL_TRANSFER) && !send_abort) {
        /* search object dictionary in first pass */
        if (SDO_C->OD_IO.read == NULL) {
            ODR_t odRet;

            odRet = OD_getSub(OD_find(SDO_C->OD, SDO_C->index), SDO_C->subIndex, &SDO_C->OD_IO, false);

            if (odRet != ODR_OK) {
                abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
                ret = CO_SDO_RT_endedWithClientAbort;
            } else if ((SDO_C->OD_IO.stream.attribute & (OD_attr_t)ODA_SDO_RW) == 0U) {
                abortCode = CO_SDO_AB_UNSUPPORTED_ACCESS;
                ret = CO_SDO_RT_endedWithClientAbort;
            } else if ((SDO_C->OD_IO.stream.attribute & (OD_attr_t)ODA_SDO_R) == 0U) {
                abortCode = CO_SDO_AB_WRITEONLY;
                ret = CO_SDO_RT_endedWithClientAbort;
            } else if (SDO_C->OD_IO.read == NULL) {
                abortCode = CO_SDO_AB_DEVICE_INCOMPAT;
                ret = CO_SDO_RT_endedWithClientAbort;
            } else { /* MISRA C 2004 14.10 */
            }
        }

        size_t countFifo = CO_fifo_getSpace(&SDO_C->bufFifo);

        /* skip copying if buffer full */
        if (countFifo == 0U) {
            ret = CO_SDO_RT_uploadDataBufferFull;
        }
        /* read data, in several passes if necessary */
        else if (SDO_C->OD_IO.read != NULL) {
            /* Get size of data in Object Dictionary. If size is not indicated
             * use maximum SDO client buffer size. Prepare temp buffer. */
            OD_size_t countData = SDO_C->OD_IO.stream.dataLength;
            OD_size_t countBuf = ((countData > 0U) && (countData <= countFifo)) ? countData : (OD_size_t)countFifo;
            OD_size_t countRd = 0;
            uint8_t buf[CO_CONFIG_SDO_CLI_BUFFER_SIZE + 1U];

            /* load data from OD variable into the buffer */
            CO_LOCK_OD(SDO_C->CANdevTx);
            ODR_t odRet = SDO_C->OD_IO.read(&SDO_C->OD_IO.stream, buf, countBuf, &countRd);
            CO_UNLOCK_OD(SDO_C->CANdevTx);

            if ((odRet != ODR_OK) && (odRet != ODR_PARTIAL)) {
                abortCode = (CO_SDO_abortCode_t)OD_getSDOabCode(odRet);
                ret = CO_SDO_RT_endedWithServerAbort;
            } else {
                /* if data is string, send only data up to null termination */
                if ((countRd > 0U) && (countRd <= CO_CONFIG_SDO_CLI_BUFFER_SIZE)
                    && ((SDO_C->OD_IO.stream.attribute & (OD_attr_t)ODA_STR) != 0U)) {
                    buf[countRd] = 0; /* (buf is one byte larger) */
                    OD_size_t countStr = (OD_size_t)strlen((char*)buf);
                    if (countStr == 0U) {
                        countStr = 1; /* no zero length */
                    }
                    if (countStr < countRd) {
                        /* string terminator found, finish read, shorten data */
                        countRd = countStr;
                        odRet = ODR_OK;
                        SDO_C->OD_IO.stream.dataLength = (OD_size_t)SDO_C->sizeTran + countRd;
                    }
                }

                (void)CO_fifo_write(&SDO_C->bufFifo, buf, countRd, NULL);
                SDO_C->sizeTran += countRd;

                /* verify if size of data uploaded is too large */
                SDO_C->sizeInd = SDO_C->OD_IO.stream.dataLength;
                if ((SDO_C->sizeInd > 0U) && (SDO_C->sizeTran > SDO_C->sizeInd)) {
                    abortCode = CO_SDO_AB_DATA_LONG;
                    ret = CO_SDO_RT_endedWithClientAbort;
                }
                /* If no more segments to be upload, finish */
                else if (odRet == ODR_OK) {
                    /* verify size of data uploaded */
                    if ((SDO_C->sizeInd > 0U) && (SDO_C->sizeTran < SDO_C->sizeInd)) {
                        abortCode = CO_SDO_AB_DATA_SHORT;
                        ret = CO_SDO_RT_endedWithClientAbort;
                    } else {
                        ret = CO_SDO_RT_ok_communicationEnd;
                    }
                } else {
                    ret = CO_SDO_RT_waitingLocalTransfer;
                }
            }
        } else { /* MISRA C 2004 14.10 */
        }

        if ((ret != CO_SDO_RT_uploadDataBufferFull) && (ret != CO_SDO_RT_waitingLocalTransfer)) {
            SDO_C->state = CO_SDO_ST_IDLE;
        }
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_TIMERNEXT) != 0
        /* Inform OS to call this function again without delay. */
        else if (timerNext_us != NULL) {
            *timerNext_us = 0;
        } else { /* MISRA C 2004 14.10 */
        }
#endif
    }
#endif /* CO_CONFIG_SDO_CLI_LOCAL */
    /* CAN data received */
    else if (CO_FLAG_READ(SDO_C->CANrxNew)) {
        /* is SDO abort */
        if (SDO_C->CANrxData[0] == 0x80U) {
            uint32_t code;
            (void)memcpy((void*)(&code), (const void*)(&SDO_C->CANrxData[4]), sizeof(code));
            abortCode = (CO_SDO_abortCode_t)CO_SWAP_32(code);
            SDO_C->state = CO_SDO_ST_IDLE;
            ret = CO_SDO_RT_endedWithServerAbort;
        } else if (send_abort) {
            abortCode = (SDOabortCode != NULL) ? *SDOabortCode : CO_SDO_AB_DEVICE_INCOMPAT;
            SDO_C->state = CO_SDO_ST_ABORT;
        } else {
            switch (SDO_C->state) {
                case CO_SDO_ST_UPLOAD_INITIATE_RSP: {
                    if ((SDO_C->CANrxData[0] & 0xF0U) == 0x40U) {
                        /* verify index and subindex */
                        uint16_t index;
                        uint8_t subindex;
                        index = ((uint16_t)SDO_C->CANrxData[2]) << 8;
                        index |= SDO_C->CANrxData[1];
                        subindex = SDO_C->CANrxData[3];
                        if ((index != SDO_C->index) || (subindex != SDO_C->subIndex)) {
                            abortCode = CO_SDO_AB_PRAM_INCOMPAT;
                            SDO_C->state = CO_SDO_ST_ABORT;
                            break;
                        }

                        if ((SDO_C->CANrxData[0] & 0x02U) != 0U) {
                            /* Expedited transfer */
                            size_t count = 4;
                            /* is size indicated? */
                            if ((SDO_C->CANrxData[0] & 0x01U) != 0U) {
                                count -= (((size_t)SDO_C->CANrxData[0]) >> 2) & 0x03U;
                            }
                            /* copy data, indicate size and finish */
                            (void)CO_fifo_write(&SDO_C->bufFifo, &SDO_C->CANrxData[4], count, NULL);
                            SDO_C->sizeTran = count;
                            SDO_C->state = CO_SDO_ST_IDLE;
                            ret = CO_SDO_RT_ok_communicationEnd;
                        } else {
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_SEGMENTED) != 0
                            /* segmented transfer, is size indicated? */
                            if ((SDO_C->CANrxData[0] & 0x01U) != 0U) {
                                uint32_t size;
                                (void)memcpy((void*)(&size), (void*)(&SDO_C->CANrxData[4]), sizeof(size));
                                SDO_C->sizeInd = CO_SWAP_32(size);
                            }
                            SDO_C->toggle = 0x00;
                            SDO_C->state = CO_SDO_ST_UPLOAD_SEGMENT_REQ;
#else
                            abortCode = CO_SDO_AB_UNSUPPORTED_ACCESS;
                            SDO_C->state = CO_SDO_ST_ABORT;
#endif
                        }
                    } else {
                        abortCode = CO_SDO_AB_CMD;
                        SDO_C->state = CO_SDO_ST_ABORT;
                    }
                    break;
                }

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_SEGMENTED) != 0
                case CO_SDO_ST_UPLOAD_SEGMENT_RSP: {
                    if ((SDO_C->CANrxData[0] & 0xE0U) == 0x00U) {
                        size_t count, countWr;

                        /* verify and alternate toggle bit */
                        uint8_t toggle = SDO_C->CANrxData[0] & 0x10U;
                        if (toggle != SDO_C->toggle) {
                            abortCode = CO_SDO_AB_TOGGLE_BIT;
                            SDO_C->state = CO_SDO_ST_ABORT;
                            break;
                        }
                        SDO_C->toggle = (toggle == 0x00U) ? 0x10U : 0x00U;

                        /* get data size and write data to the buffer */
                        count = (size_t)(7U) - (((size_t)(SDO_C->CANrxData[0]) >> 1) & 0x07U);
                        countWr = CO_fifo_write(&SDO_C->bufFifo, &SDO_C->CANrxData[1], count, NULL);
                        SDO_C->sizeTran += countWr;

                        /* verify, if there was not enough space in fifo buffer */
                        if (countWr != count) {
                            abortCode = CO_SDO_AB_OUT_OF_MEM;
                            SDO_C->state = CO_SDO_ST_ABORT;
                            break;
                        }

                        /* verify if size of data uploaded is too large */
                        if ((SDO_C->sizeInd > 0U) && (SDO_C->sizeTran > SDO_C->sizeInd)) {
                            abortCode = CO_SDO_AB_DATA_LONG;
                            SDO_C->state = CO_SDO_ST_ABORT;
                            break;
                        }

                        /* If no more segments to be upload, finish */
                        if ((SDO_C->CANrxData[0] & 0x01U) != 0U) {
                            /* verify size of data uploaded */
                            if ((SDO_C->sizeInd > 0U) && (SDO_C->sizeTran < SDO_C->sizeInd)) {
                                abortCode = CO_SDO_AB_DATA_SHORT;
                                SDO_C->state = CO_SDO_ST_ABORT;
                            } else {
                                SDO_C->state = CO_SDO_ST_IDLE;
                                ret = CO_SDO_RT_ok_communicationEnd;
                            }
                        } else {
                            SDO_C->state = CO_SDO_ST_UPLOAD_SEGMENT_REQ;
                        }
                    } else {
                        abortCode = CO_SDO_AB_CMD;
                        SDO_C->state = CO_SDO_ST_ABORT;
                    }
                    break;
                }
#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED */

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
                case CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP: {
                    if ((SDO_C->CANrxData[0] & 0xF9U) == 0xC0U) {
                        uint16_t index;
                        uint8_t subindex;

                        /* get server CRC support info and data size */
                        if ((SDO_C->CANrxData[0] & 0x04U) != 0U) {
                            SDO_C->block_crcEnabled = true;
                        } else {
                            SDO_C->block_crcEnabled = false;
                        }
                        if ((SDO_C->CANrxData[0] & 0x02U) != 0U) {
                            uint32_t size;
                            (void)memcpy((void*)(&size), (const void*)(&SDO_C->CANrxData[4]), sizeof(size));
                            SDO_C->sizeInd = CO_SWAP_32(size);
                        }

                        /* verify index and subindex */
                        index = ((uint16_t)SDO_C->CANrxData[2]) << 8;
                        index |= SDO_C->CANrxData[1];
                        subindex = SDO_C->CANrxData[3];
                        if ((index != SDO_C->index) || (subindex != SDO_C->subIndex)) {
                            abortCode = CO_SDO_AB_PRAM_INCOMPAT;
                            SDO_C->state = CO_SDO_ST_ABORT;
                        } else {
                            SDO_C->state = CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2;
                        }
                    }
                    /* switch to regular transfer, CO_SDO_ST_UPLOAD_INITIATE_RSP */
                    else if ((SDO_C->CANrxData[0] & 0xF0U) == 0x40U) {
                        /* verify index and subindex */
                        uint16_t index;
                        uint8_t subindex;
                        index = ((uint16_t)SDO_C->CANrxData[2]) << 8;
                        index |= SDO_C->CANrxData[1];
                        subindex = SDO_C->CANrxData[3];
                        if ((index != SDO_C->index) || (subindex != SDO_C->subIndex)) {
                            abortCode = CO_SDO_AB_PRAM_INCOMPAT;
                            SDO_C->state = CO_SDO_ST_ABORT;
                            break;
                        }

                        if ((SDO_C->CANrxData[0] & 0x02U) != 0U) {
                            /* Expedited transfer */
                            size_t count = 4;
                            /* is size indicated? */
                            if ((SDO_C->CANrxData[0] & 0x01U) != 0U) {
                                count -= ((size_t)(SDO_C->CANrxData[0]) >> 2) & 0x03U;
                            }
                            /* copy data, indicate size and finish */
                            (void)CO_fifo_write(&SDO_C->bufFifo, &SDO_C->CANrxData[4], count, NULL);
                            SDO_C->sizeTran = count;
                            SDO_C->state = CO_SDO_ST_IDLE;
                            ret = CO_SDO_RT_ok_communicationEnd;
                        } else {
                            /* segmented transfer, is size indicated? */
                            if ((SDO_C->CANrxData[0] & 0x01U) != 0U) {
                                uint32_t size;
                                (void)memcpy((void*)(&size), (const void*)(&SDO_C->CANrxData[4]), sizeof(size));
                                SDO_C->sizeInd = CO_SWAP_32(size);
                            }
                            SDO_C->toggle = 0x00;
                            SDO_C->state = CO_SDO_ST_UPLOAD_SEGMENT_REQ;
                        }
                    } else {
                        abortCode = CO_SDO_AB_CMD;
                        SDO_C->state = CO_SDO_ST_ABORT;
                    }
                    break;
                }

                case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ: {
                    /* data are copied directly in the receive function */
                    break;
                }

                case CO_SDO_ST_UPLOAD_BLK_END_SREQ: {
                    if ((SDO_C->CANrxData[0] & 0xE3U) == 0xC1U) {
                        /* Get number of data bytes in last segment, that do not
                         * contain data. Then copy remaining data into fifo */
                        uint8_t noData = ((SDO_C->CANrxData[0] >> 2) & 0x07U);
                        (void)CO_fifo_write(&SDO_C->bufFifo, &SDO_C->block_dataUploadLast[0], (size_t)(7U) - noData,
                                            &SDO_C->block_crc);
                        SDO_C->sizeTran += (size_t)(7U) - noData;

                        /* verify length */
                        if ((SDO_C->sizeInd > 0U) && (SDO_C->sizeTran != SDO_C->sizeInd)) {
                            abortCode = (SDO_C->sizeTran > SDO_C->sizeInd) ? CO_SDO_AB_DATA_LONG : CO_SDO_AB_DATA_SHORT;
                            SDO_C->state = CO_SDO_ST_ABORT;
                            break;
                        }

                        /* verify CRC */
                        if (SDO_C->block_crcEnabled) {
                            uint16_t crcServer;
                            crcServer = ((uint16_t)SDO_C->CANrxData[2]) << 8;
                            crcServer |= SDO_C->CANrxData[1];
                            if (crcServer != SDO_C->block_crc) {
                                abortCode = CO_SDO_AB_CRC;
                                SDO_C->state = CO_SDO_ST_ABORT;
                                break;
                            }
                        }
                        SDO_C->state = CO_SDO_ST_UPLOAD_BLK_END_CRSP;
                    } else {
                        abortCode = CO_SDO_AB_CMD;
                        SDO_C->state = CO_SDO_ST_ABORT;
                    }
                    break;
                }
#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_SEGMENTED) == 0
                case CO_SDO_ST_UPLOAD_SEGMENT_RSP:
#endif
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) == 0
                case CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP:
                case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ:
                case CO_SDO_ST_UPLOAD_BLK_END_SREQ:
#endif
                case CO_SDO_ST_IDLE:
                case CO_SDO_ST_ABORT:
                case CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER:
                case CO_SDO_ST_DOWNLOAD_INITIATE_REQ:
                case CO_SDO_ST_DOWNLOAD_INITIATE_RSP:
                case CO_SDO_ST_DOWNLOAD_SEGMENT_REQ:
                case CO_SDO_ST_DOWNLOAD_SEGMENT_RSP:
                case CO_SDO_ST_UPLOAD_LOCAL_TRANSFER:
                case CO_SDO_ST_UPLOAD_INITIATE_REQ:
                case CO_SDO_ST_UPLOAD_SEGMENT_REQ:
                case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ:
                case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP:
                case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ:
                case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP:
                case CO_SDO_ST_DOWNLOAD_BLK_END_REQ:
                case CO_SDO_ST_DOWNLOAD_BLK_END_RSP:
                case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ:
                case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2:
                case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP:
                case CO_SDO_ST_UPLOAD_BLK_END_CRSP:
                default: {
                    abortCode = CO_SDO_AB_CMD;
                    SDO_C->state = CO_SDO_ST_ABORT;
                    break;
                }
            }
        }
        SDO_C->timeoutTimer = 0;
        timeDifference_us = 0;
        CO_FLAG_CLEAR(SDO_C->CANrxNew);
    } else if (send_abort) {
        abortCode = (SDOabortCode != NULL) ? *SDOabortCode : CO_SDO_AB_DEVICE_INCOMPAT;
        SDO_C->state = CO_SDO_ST_ABORT;
    } else { /* MISRA C 2004 14.10 */
    }

    /* Timeout timers and transmit bufferFull flag */
    if (ret == CO_SDO_RT_waitingResponse) {
        if (SDO_C->timeoutTimer < SDO_C->SDOtimeoutTime_us) {
            SDO_C->timeoutTimer += timeDifference_us;
        }
        if (SDO_C->timeoutTimer >= SDO_C->SDOtimeoutTime_us) {
            bool_t state_upload_seg_req = (SDO_C->state == CO_SDO_ST_UPLOAD_SEGMENT_REQ);
            bool_t state_upload_blk_sublock_crsp = (SDO_C->state == CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP);
            if (state_upload_seg_req || state_upload_blk_sublock_crsp) {
                /* application didn't empty buffer */
                abortCode = CO_SDO_AB_GENERAL;
            } else {
                abortCode = CO_SDO_AB_TIMEOUT;
            }
            SDO_C->state = CO_SDO_ST_ABORT;
        }
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_TIMERNEXT) != 0
        else if (timerNext_us != NULL) {
            /* check again after timeout time elapsed */
            uint32_t diff = SDO_C->SDOtimeoutTime_us - SDO_C->timeoutTimer;
            if (*timerNext_us > diff) {
                *timerNext_us = diff;
            }
        } else { /* MISRA C 2004 14.10 */
        }
#endif

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
        /* Timeout for sub-block reception */
        if (SDO_C->state == CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ) {
            if (SDO_C->block_timeoutTimer < SDO_C->block_SDOtimeoutTime_us) {
                SDO_C->block_timeoutTimer += timeDifference_us;
            }
            if (SDO_C->block_timeoutTimer >= SDO_C->block_SDOtimeoutTime_us) {
                /* SDO_C->state will change, processing will continue in this
                 * thread. Make memory barrier here with CO_FLAG_CLEAR() call. */
                SDO_C->state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP;
                CO_FLAG_CLEAR(SDO_C->CANrxNew);
            }
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_TIMERNEXT) != 0
            else if (timerNext_us != NULL) {
                /* check again after timeout time elapsed */
                uint32_t diff = SDO_C->block_SDOtimeoutTime_us - SDO_C->block_timeoutTimer;
                if (*timerNext_us > diff) {
                    *timerNext_us = diff;
                }
            } else { /* MISRA C 2004 14.10 */
            }
#endif
        }
#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */

        if (SDO_C->CANtxBuff->bufferFull) {
            ret = CO_SDO_RT_transmittBufferFull;
        }
    }

    /* Transmit CAN data */
    if (ret == CO_SDO_RT_waitingResponse) {
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
        size_t count;
#endif
        (void)memset((void*)&SDO_C->CANtxBuff->data[0], 0, 8);

        switch (SDO_C->state) {
            case CO_SDO_ST_UPLOAD_INITIATE_REQ: {
                SDO_C->CANtxBuff->data[0] = 0x40;
                SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index;
                SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8);
                SDO_C->CANtxBuff->data[3] = SDO_C->subIndex;

                /* reset timeout timer and send message */
                SDO_C->timeoutTimer = 0;
                (void)CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
                SDO_C->state = CO_SDO_ST_UPLOAD_INITIATE_RSP;
                break;
            }

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_SEGMENTED) != 0
            case CO_SDO_ST_UPLOAD_SEGMENT_REQ: {
                /* verify, if there is enough space in data buffer */
                if (CO_fifo_getSpace(&SDO_C->bufFifo) < 7U) {
                    ret = CO_SDO_RT_uploadDataBufferFull;
                    break;
                }
                SDO_C->CANtxBuff->data[0] = 0x60U | SDO_C->toggle;

                /* reset timeout timer and send message */
                SDO_C->timeoutTimer = 0;
                (void)CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
                SDO_C->state = CO_SDO_ST_UPLOAD_SEGMENT_RSP;
                break;
            }
#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_SEGMENTED */

#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
            case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ: {
                SDO_C->CANtxBuff->data[0] = 0xA4;
                SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index;
                SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8);
                SDO_C->CANtxBuff->data[3] = SDO_C->subIndex;

                /* calculate number of block segments from free buffer space */
                count = CO_fifo_getSpace(&SDO_C->bufFifo) / 7U;
                if (count > 127U) {
                    count = 127;
                } else if (count == 0U) {
                    abortCode = CO_SDO_AB_OUT_OF_MEM;
                    SDO_C->state = CO_SDO_ST_ABORT;
                    break;
                } else { /* MISRA C 2004 14.10 */
                }
                SDO_C->block_blksize = (uint8_t)count;
                SDO_C->CANtxBuff->data[4] = SDO_C->block_blksize;
                SDO_C->CANtxBuff->data[5] = CO_CONFIG_SDO_CLI_PST;

                /* reset timeout timer and send message */
                SDO_C->timeoutTimer = 0;
                (void)CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
                SDO_C->state = CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP;
                break;
            }

            case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2: {
                SDO_C->CANtxBuff->data[0] = 0xA3;

                /* reset timeout timers, seqno and send message */
                SDO_C->timeoutTimer = 0;
                SDO_C->block_timeoutTimer = 0;
                SDO_C->block_seqno = 0;
                SDO_C->block_crc = 0;
                /* Block segments will be received in different thread. Make memory
                 * barrier here with CO_FLAG_CLEAR() call. */
                SDO_C->state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ;
                CO_FLAG_CLEAR(SDO_C->CANrxNew);
                (void)CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
                break;
            }

            case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP: {
                SDO_C->CANtxBuff->data[0] = 0xA2;
                SDO_C->CANtxBuff->data[1] = SDO_C->block_seqno;
#ifdef CO_DEBUG_SDO_CLIENT
                bool_t transferShort = SDO_C->block_seqno != SDO_C->block_blksize;
                uint8_t seqnoStart = SDO_C->block_seqno;
#endif

                /* Is last segment? */
                if (SDO_C->finished) {
                    SDO_C->state = CO_SDO_ST_UPLOAD_BLK_END_SREQ;
                } else {
                    /* verify if size of data uploaded is too large */
                    if ((SDO_C->sizeInd > 0U) && (SDO_C->sizeTran > SDO_C->sizeInd)) {
                        abortCode = CO_SDO_AB_DATA_LONG;
                        SDO_C->state = CO_SDO_ST_ABORT;
                        break;
                    }

                    /* calculate number of block segments from free buffer space */
                    count = CO_fifo_getSpace(&SDO_C->bufFifo) / 7U;
                    if (count >= 127U) {
                        count = 127;
                    } else if (CO_fifo_getOccupied(&SDO_C->bufFifo) > 0U) {
                        /* application must empty data buffer first */
                        ret = CO_SDO_RT_uploadDataBufferFull;
#ifdef CO_DEBUG_SDO_CLIENT
                        if (transferShort) {
                            char msg[80];
                            sprintf(msg, "sub-block, uploadDataBufferFull: sequno=%02X", seqnoStart);
                            CO_DEBUG_SDO_CLIENT(msg);
                        }
#endif
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_FLAG_TIMERNEXT) != 0
                        /* Inform OS to call this function again without delay. */
                        if (timerNext_us != NULL) {
                            *timerNext_us = 0;
                        }
#endif
                        break;
                    } else { /* MISRA C 2004 14.10 */
                    }

                    SDO_C->block_blksize = (uint8_t)count;
                    SDO_C->block_seqno = 0;
                    /* Block segments will be received in different thread. Make
                     * memory barrier here with CO_FLAG_CLEAR() call. */
                    SDO_C->state = CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ;
                    CO_FLAG_CLEAR(SDO_C->CANrxNew);
                }

                SDO_C->CANtxBuff->data[2] = SDO_C->block_blksize;

                /* reset block_timeoutTimer, but not SDO_C->timeoutTimer */
                SDO_C->block_timeoutTimer = 0;
                (void)CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
#ifdef CO_DEBUG_SDO_CLIENT
                if (transferShort && !SDO_C->finished) {
                    char msg[80];
                    sprintf(msg, "sub-block restarted: sequnoPrev=%02X, blksize=%02X", seqnoStart,
                            SDO_C->block_blksize);
                    CO_DEBUG_SDO_CLIENT(msg);
                }
#endif
                break;
            }

            case CO_SDO_ST_UPLOAD_BLK_END_CRSP: {
                SDO_C->CANtxBuff->data[0] = 0xA1;

                (void)CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
                SDO_C->state = CO_SDO_ST_IDLE;
                ret = CO_SDO_RT_ok_communicationEnd;
                break;
            }
#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_BLOCK */
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_SEGMENTED) == 0
            case CO_SDO_ST_UPLOAD_SEGMENT_REQ:
#endif
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) == 0
            case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ:
            case CO_SDO_ST_UPLOAD_BLK_INITIATE_REQ2:
            case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_CRSP:
            case CO_SDO_ST_UPLOAD_BLK_END_CRSP:
#endif
            case CO_SDO_ST_IDLE:
            case CO_SDO_ST_ABORT:
            case CO_SDO_ST_DOWNLOAD_LOCAL_TRANSFER:
            case CO_SDO_ST_DOWNLOAD_INITIATE_REQ:
            case CO_SDO_ST_DOWNLOAD_INITIATE_RSP:
            case CO_SDO_ST_DOWNLOAD_SEGMENT_REQ:
            case CO_SDO_ST_DOWNLOAD_SEGMENT_RSP:
            case CO_SDO_ST_UPLOAD_LOCAL_TRANSFER:
            case CO_SDO_ST_UPLOAD_INITIATE_RSP:
            case CO_SDO_ST_UPLOAD_SEGMENT_RSP:
            case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_REQ:
            case CO_SDO_ST_DOWNLOAD_BLK_INITIATE_RSP:
            case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_REQ:
            case CO_SDO_ST_DOWNLOAD_BLK_SUBBLOCK_RSP:
            case CO_SDO_ST_DOWNLOAD_BLK_END_REQ:
            case CO_SDO_ST_DOWNLOAD_BLK_END_RSP:
            case CO_SDO_ST_UPLOAD_BLK_INITIATE_RSP:
            case CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ:
            case CO_SDO_ST_UPLOAD_BLK_END_SREQ:
            default: {
                /* none */
                break;
            }
        }
    }

    if (ret == CO_SDO_RT_waitingResponse) {
        if (SDO_C->state == CO_SDO_ST_ABORT) {
            uint32_t code = CO_SWAP_32((uint32_t)abortCode);
            /* Send SDO abort message */
            SDO_C->CANtxBuff->data[0] = 0x80;
            SDO_C->CANtxBuff->data[1] = (uint8_t)SDO_C->index;
            SDO_C->CANtxBuff->data[2] = (uint8_t)(SDO_C->index >> 8);
            SDO_C->CANtxBuff->data[3] = SDO_C->subIndex;

            (void)memcpy((void*)(&SDO_C->CANtxBuff->data[4]), (const void*)(&code), sizeof(code));
            (void)CO_CANsend(SDO_C->CANdevTx, SDO_C->CANtxBuff);
            SDO_C->state = CO_SDO_ST_IDLE;
            ret = CO_SDO_RT_endedWithClientAbort;
        }
#if ((CO_CONFIG_SDO_CLI)&CO_CONFIG_SDO_CLI_BLOCK) != 0
        else if (SDO_C->state == CO_SDO_ST_UPLOAD_BLK_SUBBLOCK_SREQ) {
            ret = CO_SDO_RT_blockUploadInProgress;
        } else { /* MISRA C 2004 14.10 */
        }
#endif
    }

    if (sizeIndicated != NULL) {
        *sizeIndicated = SDO_C->sizeInd;
    }
    if (sizeTransferred != NULL) {
        *sizeTransferred = SDO_C->sizeTran;
    }
    if (SDOabortCode != NULL) {
        *SDOabortCode = abortCode;
    }

    return ret;
}

size_t
CO_SDOclientUploadBufRead(CO_SDOclient_t* SDO_C, uint8_t* buf, size_t count) {
    size_t ret = 0;
    if ((SDO_C != NULL) && (buf != NULL)) {
        ret = CO_fifo_read(&SDO_C->bufFifo, buf, count, NULL);
    }
    return ret;
}

void
CO_SDOclientClose(CO_SDOclient_t* SDO_C) {
    if (SDO_C != NULL) {
        SDO_C->state = CO_SDO_ST_IDLE;
    }
}

#endif /* (CO_CONFIG_SDO_CLI) & CO_CONFIG_SDO_CLI_ENABLE */
